#! /usr/bin/env python3
import rospy
import roslaunch
import rosgraph
import os
import actionlib
from reinovo_web.msg import startup_nodeAction, startup_nodeFeedback, startup_nodeResult,SN_state
from reinovo_web.srv import SN_cmd,SN_cmdResponse
from multiprocessing import Process,Queue

# parma_server = rosgraph.Master('/roslaunch')
# var = parma_server.getParam('/run_id')


def add_launch(object_list,pkg,file,args=[]):
    """
    查找launch路径
    :param object_list: 启动列表，将在函数内修改
    :param pkg: 功能包名称
    :param file: launch文件名
    :param arg: launch的可选参数配置列表,["param1 := num1", "param2:=num2"] 
    :return: 添加是否成功
    """
    ros_path = os.environ['ROS_PACKAGE_PATH']
    cli_args = []
    for dir in ros_path.split(':'):
        path = os.path.join(dir,pkg)
        if os.path.exists(path):
            path = os.path.join(os.path.join(path,'launch'),file)
            if os.path.exists(path):
                cli_args.append(path)
                if len(args) == 0:
                    object_list.append(path)
                else:
                    for arg in args:
                        cli_args.append(arg)
                    object_list.append((roslaunch.rlutil.resolve_launch_arguments(cli_args)[0],cli_args[1:]))
                return True
            else:
                rospy.logerr('未找到launch文件,pkg:'+pkg+',file:'+file)
                return False
    
def create_launchList(launch,is_core=False):
    launchList = []
    uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
    for code in launch:
        add_launch(launchList,code.split(' ')[1],code.split(' ')[2],code.split(' ')[3:])
    rosbridge_launch = roslaunch.parent.ROSLaunchParent(uuid,launchList,is_core=is_core)
    return rosbridge_launch     

class StatupNode():
    def __init__(self,name):
        # self.start_launch = create_launchList(['roslaunch rosbridge_server rosbridge_websocket.launch'],is_core=True)
        # self.start_launch.start()
        self._action_name = name
        rospy.init_node('statupNode',disable_signals=False,anonymous=False)
        print(self._action_name)
        self._as = actionlib.SimpleActionServer(self._action_name,startup_nodeAction,execute_cb=self.execute_cb, auto_start=False)
        self._as.start()
        self.close_launch= Queue()
        self.close_start_launch = Queue()
        self._proceList = []
        self._service = rospy.Service(name+'/close',SN_cmd,self.cmd_cb)
        self._state = SN_state.CLOSE

        # start_launch = Process(target=self._start,args=(2,))
        # start_launch.start()
    
    def _start(self,param):
        launch = create_launchList(['roslaunch rosbridge_server rosbridge_websocket.launch'],is_core=True)
        launch.start()
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            cmd = self.close_start_launch.get()
            if cmd=='close':
                launch.shutdown()
                break
            rate.sleep()
        
    def cmd_cb(self, req):
        # if self._state == SN_state.ACTIVE:
        for i in self._proceList:
            self.close_launch.put("close")
        self._state = SN_state.PENDING
        return SN_cmdResponse(True,str(self._state))
    
    def run_launch_start(self,launchList):
        launchList.start()
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            cmd = self.close_launch.get()
            if cmd=='close':
                break
            rate.sleep()
        print("---------------close------------------")
        launchList.shutdown()
        print("---------------success------------------")
        
    def execute_cb(self, goal):
        """
        待机-》启动中-》运行中-》关闭中-》待机
        """  
        self.launchList = create_launchList(goal.roslaunch)
        result = startup_nodeResult()
        feedback = startup_nodeFeedback()
        target_launch = Process(target=self.run_launch_start,args=(self.launchList,))
        self._proceList.append(target_launch)
        target_launch.start()
        self._state = SN_state.ACTIVE 
        feedback.run_state = self._state
        self._as.publish_feedback(feedback)
        result.result = True
        self._as.set_succeeded(result)

    def close(self):
        for i in self._proceList:
            self.close_launch.put("close")
            i.join()
        
        # self.close_start_launch.put("close")
        # self.start_launch.shutdown()
        
if __name__ == '__main__':
    # rospy.init_node('statupNode',disable_signals=False,anonymous=False)
    server = StatupNode('statupNode')
    rospy.spin()
    server.close()